﻿/*
 * ©2009-2012 XELF
 * http://xelf.info
 */

namespace XELF.Framework {
	public partial struct DualQuaternion {

		/// <summary>
		/// 対数
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void Log(ref DualQuaternion a, out DualQuaternion result) {
			var vv = a.Real.X * a.Real.X + a.Real.Y * a.Real.Y + a.Real.Z * a.Real.Z;
			var vd = a.Real.X * a.Dual.X + a.Real.Y * a.Dual.Y + a.Real.Z * a.Dual.Z;
			var vl = MathHelper.Sqrt(vv);
			var ll = vv + a.Real.W * a.Real.W;
			var l = MathHelper.Sqrt(ll);
			var rw = a.Real.W;
			var dw = a.Dual.W;
			var rl = 1.0f / l;
			var rlrl = rl * rl;
			var rwrl = rw * rl;
			var rv = vl > 0 ? 1.0f / vl : 1.0f;
			var acos = MathHelper.Acos(rwrl);
			var arv = acos * rv;
			var rs = (vl > 0 && rwrl < 1.0f) ? MathHelper.Rsqrt(1 - rwrl * rwrl) : 1.0f;
			var d = vd + a.Real.W * a.Dual.W;
			var t = vl > 0 ? acos / vv : 0.0f;
			var u = (vd * t + (dw * l - rw * d * rl) * rlrl * rs) * rv;
			var vlt = vl * t;
			result = new DualQuaternion(
				a.Real.X * arv,
				a.Real.Y * arv,
				a.Real.Z * arv,
				MathHelper.Log(l),
				a.Dual.X * vlt - a.Real.X * u,
				a.Dual.Y * vlt - a.Real.Y * u,
				a.Dual.Z * vlt - a.Real.Z * u,
				d * rlrl);
		}

		/// <summary>
		/// 指数
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void Exp(ref DualQuaternion a, out DualQuaternion result) {
			var vv = a.Real.X * a.Real.X + a.Real.Y * a.Real.Y + a.Real.Z * a.Real.Z;
			var vl = MathHelper.Sqrt(vv);

			var r = a.Real.LengthSquared();
			float rv, v;
			if (r != 0) {
				r = MathHelper.Sqrt(r);
				if (vl == 0)
					rv = 1;
				else
					rv = 1.0f / vl;
			} else {
				rv = 1.0f;
			}
			if (r == 0) {
				v = 1.0f;
			} else {
				v = vl;
			}
			var d = (a.Real.X * a.Dual.X + a.Real.Y * a.Dual.Y + a.Real.Z * a.Dual.Z) * rv;
			var e = MathHelper.Exp(a.Real.W);
			var c = e * MathHelper.Cos(vl);
			var s = e * MathHelper.Sin(vl);
			var rvs = rv * s;
			var t = rv * rvs;
			var u = rv * (a.Dual.W * s + d * c) - d * t;
			result = new DualQuaternion(
				a.Real.X * rvs,
				a.Real.Y * rvs,
				a.Real.Z * rvs,
				c,
				a.Dual.X * rvs + a.Real.X * u,
				a.Dual.Y * rvs + a.Real.Y * u,
				a.Dual.Z * rvs + a.Real.Z * u,
				a.Dual.W * c - d * s);
		}

		/// <summary>
		/// Squad(Spherical Quadraple)補間を計算する。
		/// </summary>
		/// <param name="b">キーb</param>
		/// <param name="c">キーc</param>
		/// <param name="tangentB">bの接線</param>
		/// <param name="tangentC">cの接線</param>
		/// <param name="amount">0～1。0のときキーbを、1のときキーcを通る。</param>
		/// <param name="result">結果</param>
		public static void Squad(
			ref DualQuaternion b, ref DualQuaternion c, ref DualQuaternion tangentB, ref DualQuaternion tangentC,
			float amount, out DualQuaternion result) {
			DualQuaternion bc, tbc;
			Sclerp(ref b, ref c, amount, out bc);
			Sclerp(ref tangentB, ref tangentC, amount, out tbc);
			Sclerp(ref bc, ref tbc, 2 * amount * (1 - amount), out result);
		}
		/// <summary>
		/// キーのシーケンスa, b, cのbにおける接線を計算する。
		/// </summary>
		/// <param name="a">キーa</param>
		/// <param name="b">キーb</param>
		/// <param name="c">キーc</param>
		/// <param name="result">結果</param>
		public static void Tangent(ref DualQuaternion a, ref DualQuaternion b, ref DualQuaternion c, out DualQuaternion result) {
			//result = b * exp(-0.25f * (log(b^-1 a) + log(b^-1 c)));
			DualQuaternion ib, ba, bc;
			Invert(ref b, out ib);
			Multiply(ref ib, ref a, out ba);
			Multiply(ref ib, ref c, out bc);
			Log(ref ba, out ba);
			Log(ref bc, out bc);
			Add(ref ba, ref bc, out result);
			Multiply(ref result, -0.25f, out result);
			Exp(ref result, out result);
			result.Normalize();
			Multiply(ref b, ref result, out result);
		}

		/// <summary>
		/// キーのシーケンスa, b, c, dから、Squad用のbとcの接線を計算する。
		/// </summary>
		/// <param name="a">キーa</param>
		/// <param name="b">キーb</param>
		/// <param name="c">キーc</param>
		/// <param name="d">キーd</param>
		/// <param name="tangentB">b接線</param>
		/// <param name="tangentC">c接線</param>
		public static void SetupSquad(
			ref DualQuaternion a, ref DualQuaternion b, ref DualQuaternion c, ref DualQuaternion d,
			out DualQuaternion tangentB, out DualQuaternion tangentC) {
			Tangent(ref a, ref b, ref c, out tangentB);
			Tangent(ref b, ref c, ref d, out tangentC);
		}

		/// <summary>
		/// キーのシーケンスa, b, c, dによるSquad(Spherical Quadraple)補間をする。0のときキーbを通る。1のときキーcを通る。
		/// </summary>
		/// <param name="a">キーa</param>
		/// <param name="b">キーb</param>
		/// <param name="c">キーc</param>
		/// <param name="d">キーd</param>
		/// <param name="amount">0～1</param>
		/// <param name="result">結果</param>
		public static void SquadBlend(
			ref DualQuaternion a, ref DualQuaternion b, ref DualQuaternion c, ref DualQuaternion d,
			float amount, out DualQuaternion result) {
			DualQuaternion tangentB, tangentC;
			SetupSquad(ref a, ref b, ref c, ref d, out tangentB, out tangentC);
			Squad(ref b, ref c, ref tangentB, ref tangentC, amount, out result);
		}

	}

}
